from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
import os
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    # 获取包的共享目录
    # pkg_share = get_package_share_directory('one_line_lidar_mapping')

    gps_kalman_filter_node = Node(
        package='robot_localization',
        executable='gps_kalman_filter_node',
        name='gps_kalman_filter_node',
        output='screen',
        parameters=[{
            "lat.process_noise": 1e-7,  # 纬度的过程噪声
            "lat.measure_noise": 1e-6,  # 纬度的测量噪声
            "lat.estimate_error": 1.0,  # 纬度的估计误差
            "lat.init_value": 0.0,  # 纬度的初始值
            "lon.process_noise": 1e-7,  # 经度的过程噪声
            "lon.measure_noise": 1e-6,  # 经度的测量噪声
            "lon.estimate_error": 1.0,  # 经度的估计误差
            "lon.init_value": 0.0,  # 经度的初始值
            "alt.process_noise": 1e-4,  # 高度的过程噪声
            "alt.measure_noise": 1e-2,  # 高度的测量噪声
            "alt.estimate_error": 1.0,  # 高度的估计误差
            "alt.init_value": 0.0,  # 高度的初始值
        }],
    )

    # 返回启动描述
    return LaunchDescription([
        gps_kalman_filter_node,
    ])
